#include "commander.h"

pthread_mutex_t mission_ack_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t command_ack_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t ask_flag_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t heartbeat_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mission_ack_route_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mission_ack_eask_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t route_flag_lock = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t taff_flag_lock = PTHREAD_MUTEX_INITIALIZER;



/****************Commander Class***************************/
xy_command_c::
xy_command_c(autopilot_interface_c &_pilot):pilot(_pilot)
{

	ask_flag = 0;
	route_flag = 0;

}

xy_command_c::
~xy_command_c()
{}


void 
xy_command_c::
msg_pack(udp_pack *udp)
{
	int time_s= time((time_t*)NULL);
	printf("time is %ds\n", time_s);
	udp->cur_battery.msg_id = 2;
	udp->cur_battery.id_uav_xyi = XYI_UAV_ID(pilot.current_messages.sysid);
	udp->cur_battery.time_boot_ms = time_s;
	memcpy(udp->cur_battery.voltages, pilot.current_messages.battery_status.voltages, 10);
	udp->cur_battery.voltages[0] = pilot.current_messages.sys_status.voltage_battery;
	udp->cur_battery.current_battery = pilot.current_messages.battery_status.current_battery;
	udp->cur_battery.battery_remaining = pilot.current_messages.battery_status.battery_remaining;

	
	udp->cur_local_pos.msg_id = 3;
	udp->cur_local_pos.id_uav_xyi = XYI_UAV_ID(pilot.current_messages.sysid);
	udp->cur_local_pos.time_boot_ms = time_s;
	udp->cur_local_pos.x = pilot.current_messages.local_position_ned.x;
	udp->cur_local_pos.y = pilot.current_messages.local_position_ned.y;
	udp->cur_local_pos.z = pilot.current_messages.local_position_ned.z;
	udp->cur_local_pos.vx = pilot.current_messages.local_position_ned.vx;
	udp->cur_local_pos.vy = pilot.current_messages.local_position_ned.vy;
	udp->cur_local_pos.vz = pilot.current_messages.local_position_ned.vz;
	udp->cur_local_pos.dis_m = 0;

	udp->cur_glob_pos.msg_id = 4;
	udp->cur_glob_pos.id_uav_xyi = XYI_UAV_ID(pilot.current_messages.sysid);
	udp->cur_glob_pos.time_boot_ms = time_s;
	udp->cur_glob_pos.relative_alt = pilot.current_messages.global_position_int.relative_alt;  // web /1000
	udp->cur_glob_pos.hdg = pilot.current_messages.global_position_int.hdg;  // web /100

	udp->cur_gps.msg_id = 5;
	udp->cur_gps.id_uav_xyi = XYI_UAV_ID(pilot.current_messages.sysid);
	udp->cur_gps.time_boot_ms = time_s;
	udp->cur_gps.fix_type = pilot.current_messages.gps_raw_int.fix_type;
	udp->cur_gps.lat_gps = (int)((((float)pilot.current_messages.gps_raw_int.lat/10000000) + GD2GE_LATI_DIFF) * 10000000);
	udp->cur_gps.lon_gps = (int)((((float)pilot.current_messages.gps_raw_int.lon/10000000) + GD2GE_LONGTI_DIFF) * 10000000);
	udp->cur_gps.alt_gps = pilot.current_messages.gps_raw_int.alt;
	udp->cur_gps.eph = pilot.current_messages.gps_raw_int.eph;
	udp->cur_gps.epv = pilot.current_messages.gps_raw_int.epv;
	udp->cur_gps.vel_gps = pilot.current_messages.gps_raw_int.vel;  // web /100
	udp->cur_gps.cog = pilot.current_messages.gps_raw_int.cog;    // web /100
	udp->cur_gps.satellites_visible = pilot.current_messages.gps_raw_int.satellites_visible;
}


/**************************xy network Class******************************/
xy_network_c::
xy_network_c(autopilot_interface_c &_pilot):mission_command(_pilot)
{
	new_link = 0;
	reading_status = 0;      // whether the read thread is running
	writing_status = 0;      // whether the write thread is running
	read_tid  = 0; // read thread id
	write_tid = 0; // write thread id
}

xy_network_c::
~xy_network_c()
{}

void 
xy_network_c::
start()
{
	if( XY_Net_Reported_Setup() < 0)
	{
		printf("XY_Net_Reported_Setup Function Open ERROR...\n");
	}

}



int 
xy_network_c::
XY_Net_Reported_Setup(void)
{
	if(sem_init(&report_drone_pos_sem, 0, 0) != 0)
		return -1;
	
	if(XY_Create_Thread(&start_network_interface_send_thread, this, THREADS_MSG_POST, -1, SCHED_RR, 94) < 0)
	{
		printf("Create Http Post Thread Error.\n");
		return -1;
	}
	return 0;
}

int
xy_network_c::
udp_report(int *sock, struct sockaddr_in *gcAddr)
{
	int bytes_sent = 0;
	udp_pack udp;
	mission_command.msg_pack(&udp);
	printf("lat-%d,   lon-%d  \n",udp.cur_gps.lat_gps, udp.cur_gps.lon_gps);
	bytes_sent+= sendto(*sock, &udp.cur_battery, sizeof(udp.cur_battery), 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));
	bytes_sent+= sendto(*sock, &udp.cur_glob_pos, sizeof(udp.cur_glob_pos), 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));
	bytes_sent+= sendto(*sock, &udp.cur_gps, sizeof(udp.cur_gps), 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));
	bytes_sent+= sendto(*sock, &udp.cur_local_pos, sizeof(udp.cur_local_pos), 0, (struct sockaddr*)gcAddr, sizeof(struct sockaddr_in));
	if(bytes_sent == 0)
	{
		printf("udp_report_faild");
	}
	return 0;
}


// ------------------------------------------------------------------------------
//   Write Thread
// ------------------------------------------------------------------------------
void
xy_network_c::
start_send_thread(void)
{
	if ( not writing_status == false )
	{
		fprintf(stderr,"write thread already running\n");
		return;
	}

	else
	{
		reported_data_thread_func();
		return;
	}

}


void 
xy_network_c::
reported_data_thread_func(void)
{
	printf("start\n");
	int ret = 0;

	char target_ip[100];

	struct sockaddr_in gcAddr; 

	int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	strcpy(target_ip, SERVER_NAME);

	int nZero = 0;
	setsockopt(sock_fd,SOL_SOCKET,SO_SNDBUF,(char *)&nZero,sizeof(nZero));
	
	memset(&gcAddr, 0, sizeof(gcAddr));
	gcAddr.sin_family = AF_INET;
	gcAddr.sin_addr.s_addr = inet_addr(target_ip);
	gcAddr.sin_port = htons(SERVER_PORT);

	mission_command.cur_heartbeat.msg_id = 1;
	mission_command.cur_heartbeat.id_uav_xyi = XYI_UAV_ID(mission_command.pilot.current_messages.sysid);
	mission_command.cur_heartbeat.id_iso_xyi = XYI_UAV_SYSTEM;
	mission_command.cur_heartbeat.base_mode = SYS_STAT_ON_LINE;
	mission_command.cur_heartbeat.system_status = 0;
	mission_command.cur_heartbeat.xylink_version = 1;
	memset(http_post_str, 0, HTTP_POST_DATA_SIZE);

	writing_status = 2;
	
	while(1)
	{		
		udp_report(&sock, &gcAddr);
		usleep(1000000);	// 1s			
	}
	writing_status = false;
	
//_exit:

	pthread_exit(NULL);
}


void*
start_network_interface_send_thread(void *args)
{
	// takes an autopilot object argument
	xy_network_c *xy_network = (xy_network_c *)args;

	// run the object's read thread
	xy_network->start_send_thread();

	// done!
	return NULL;
}




